/*
 * accel functions
 *
 */
#include "config.h"
#include "ebike-controller.h"
#include <WProgram.h>
#include <math.h>




static float lastavg =0.0;
static float maxavg  =0.0;

float controller_onoff(float avg, float angle,float time, float laststate){
   float state=0;
   
   //
   //compute the avg value
   //
   
   if((time-lastavg)>300 ){
     lastavg=time;
     maxavg=0.0;
   }
   
   if(angle<0){
     lastavg=time;
   }
   
   if(avg> maxavg){
     maxavg=avg;
     lastavg=time;
   }

   //
   // compute the power on value
   //   
   if((angle-maxavg)>3){
     return 10;
   }
   
   state = laststate-1;
   if (state<0){
     return 0;
   }
      
   // 
   // force shutdown power 
   //
   if ((angle-maxavg<-3) || (angle<-6)){   
     return 0;
   }
   
   return state;
}

